#ifndef TF2_NODE_H
#define TF2_NODE_H

#include <ros/ros.h>
// tf2中不需要tf/tf.h
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <Eigen/Dense>

#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/static_transform_broadcaster.h> // 静态广播器
#include <tf2_ros/transform_broadcaster.h>

#include <sensor_msgs/Image.h>

#include <geometry_msgs/PoseStamped.h>  // 轨迹
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>

// #include <tf/tf.h>
// #include <tf/tfMessage.h>

class TF2Node
{
public:
    tf2::Quaternion q_tf_;
    Eigen::Quaterniond q_eigen_;

    ros::NodeHandle n_;
    ros::Subscriber sub_rgb_;
    ros::Publisher pub_path_;
    nav_msgs::PathPtr msg_path_;
    int i = 0;


    geometry_msgs::TransformStamped tf_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    
    ros::Subscriber sub_imu_;
    bool first_imu_, first_flag;
    Eigen::Vector3d a0_, w0_;
    Eigen::Vector3d euler_a_, euler_w_, euler_w0_;
    // Eigen::Quaterniond q_a_, q_w_, q0_;
    double current_time_, last_time_;
    Eigen::Quaterniond q0_, q_w_;

    double t0_ = 0;

    TF2Node();
    ~TF2Node(){};
    void pubTransforms();
    void callback(const sensor_msgs::ImageConstPtr &msg_img);
    void callbackImu(const sensor_msgs::ImuConstPtr &msg_imu);
    void imuProcess(Eigen::Vector3d &a, Eigen::Vector3d &w,const double &time);
};

#endif